#include <stdio.h>

#include "lindes.h"

static void DesMem(char *, int, int);
static void contract(char *, char *);
static void dedes(char *, char *);
//static void endes(char *, char *);
static void expand(const char *, char *);
static void f(char *, int, char *);
static int getcomp(int, int);
static void iter(int, char *, char *);
static void kinit(char *);
static void p32init(void);
static void perm32(char *, char *);
static void perminit(char [][16][8], const char *);
static void permute(char *, char [][16][8], char *);
static void sinit(void);

static char kn[16][6];

static char iperm[16][16][8];
static const char ip[64] = {0x3a, 0x32, 0x2a, 0x22, 0x1a, 0x12, 0x0a, 0x02,
					0x3c, 0x34, 0x2c, 0x24, 0x1c, 0x14, 0x0c, 0x04,
					0x3e, 0x36, 0x2e, 0x26, 0x1e, 0x16, 0x0e, 0x06,
					0x40, 0x38, 0x30, 0x28, 0x20, 0x18, 0x10, 0x08,
					0x39, 0x31, 0x29, 0x21, 0x19, 0x11, 0x09, 0x01,
					0x3b, 0x33, 0x2b, 0x23, 0x1b, 0x13, 0x0b, 0x03,
					0x3d, 0x35, 0x2d, 0x25, 0x1d, 0x15, 0x0d, 0x05,
					0x3f, 0x37, 0x2f, 0x27, 0x1f, 0x17, 0x0f, 0x07};

static char fperm[16][16][8];
static const char fp[64] = {0x28, 0x08, 0x30, 0x10, 0x38, 0x18, 0x40, 0x20,
					0x27, 0x07, 0x2f, 0x0f, 0x37, 0x17, 0x3f, 0x1f,
					0x26, 0x06, 0x2e, 0x0e, 0x36, 0x16, 0x3e, 0x1e,
					0x25, 0x05, 0x2d, 0x0d, 0x35, 0x15, 0x3d, 0x1d,
					0x24, 0x04, 0x2c, 0x0c, 0x34, 0x14, 0x3c, 0x1c,
					0x23, 0x03, 0x2b, 0x0b, 0x33, 0x13, 0x3b, 0x1b,
					0x22, 0x02, 0x2a, 0x0a, 0x32, 0x12, 0x3a, 0x1a,
					0x21, 0x01, 0x29, 0x09, 0x31, 0x11, 0x39, 0x19};
					
static char p32[4][256][4];
static const char p32i[32] = {0x10, 0x07, 0x14, 0x15, 0x1d, 0x0c, 0x1c, 0x11,
							0x01, 0x0f, 0x17, 0x1a, 0x05, 0x12, 0x1f, 0x0a,
							0x02, 0x08, 0x18, 0x0e, 0x20, 0x1b, 0x03, 0x09,
							0x13, 0x0d, 0x1e, 0x06, 0x16, 0x0b, 0x04, 0x19};

static char pc1m[56];
static char pc1[56] =  {0x39, 0x31, 0x29, 0x21, 0x19, 0x11, 0x09, 0x01,
						0x3a, 0x32, 0x2a, 0x22, 0x1a, 0x12, 0x0a, 0x02,
						0x3b, 0x33, 0x2b, 0x23, 0x1b, 0x13, 0x0b, 0x03,
						0x3c, 0x34, 0x2c, 0x24, 0x3f, 0x37, 0x2f, 0x27,
						0x1f, 0x17, 0x0f, 0x07, 0x3e, 0x36, 0x2e, 0x26,
						0x1e, 0x16, 0x0e, 0x06, 0x3d, 0x35, 0x2d, 0x25,
						0x1d, 0x15, 0x0d, 0x05, 0x1c, 0x14, 0x0c, 0x04};
					
static char pcr[56];
static char pc2[48] =  {0x0e, 0x11, 0x0b, 0x18, 0x01, 0x05, 0x03, 0x1c,
						0x0f, 0x06, 0x15, 0x0a, 0x17, 0x13, 0x0c, 0x04,
						0x1a, 0x08, 0x10, 0x07, 0x1b, 0x14, 0x0d, 0x02,
						0x29, 0x34, 0x1f, 0x25, 0x2f, 0x37, 0x1e, 0x28,
						0x33, 0x2d, 0x21, 0x30, 0x2c, 0x31, 0x27, 0x38,
						0x22, 0x35, 0x2e, 0x2a, 0x32, 0x24, 0x1d, 0x20};
						
static int s[4][4096];
static const int si[8][64] = {
	{0x0e, 0x04, 0x0d, 0x01, 0x02, 0x0f, 0x0b, 0x08,
	 0x03, 0x0a, 0x06, 0x0c, 0x05, 0x09, 0x00, 0x07,
	 0x00, 0x0f, 0x07, 0x04, 0x0e, 0x02, 0x0d, 0x01,
	 0x0a, 0x06, 0x0c, 0x0b, 0x09, 0x05, 0x03, 0x08,
	 0x04, 0x01, 0x0e, 0x08, 0x0d, 0x06, 0x02, 0x0b,
	 0x0f, 0x0c, 0x09, 0x07, 0x03, 0x0a, 0x05, 0x00,
	 0x0f, 0x0c, 0x08, 0x02, 0x04, 0x09, 0x01, 0x07,
	 0x05, 0x0b, 0x03, 0x0e, 0x0a, 0x00, 0x06, 0x0d},
	{0x0f, 0x01, 0x08, 0x0e, 0x06, 0x0b, 0x03, 0x04,
	 0x09, 0x07, 0x02, 0x0d, 0x0c, 0x00, 0x05, 0x0a,
	 0x03, 0x0d, 0x04, 0x07, 0x0f, 0x02, 0x08, 0x0e,
	 0x0c, 0x00, 0x01, 0x0a, 0x06, 0x09, 0x0b, 0x05,
	 0x00, 0x0e, 0x07, 0x0b, 0x0a, 0x04, 0x0d, 0x01,
	 0x05, 0x08, 0x0c, 0x06, 0x09, 0x03, 0x02, 0x0f,
	 0x0d, 0x08, 0x0a, 0x01, 0x03, 0x0f, 0x04, 0x02,
	 0x0b, 0x06, 0x07, 0x0c, 0x00, 0x05, 0x0e, 0x09},
	{0x0a, 0x00, 0x09, 0x0e, 0x06, 0x03, 0x0f, 0x05,
	 0x01, 0x0d, 0x0c, 0x07, 0x0b, 0x04, 0x02, 0x08,
	 0x0d, 0x07, 0x00, 0x09, 0x03, 0x04, 0x06, 0x0a,
	 0x02, 0x08, 0x05, 0x0e, 0x0c, 0x0b, 0x0f, 0x01,
	 0x0d, 0x06, 0x04, 0x09, 0x08, 0x0f, 0x03, 0x00,
	 0x0b, 0x01, 0x02, 0x0c, 0x05, 0x0a, 0x0e, 0x07,
	 0x01, 0x0a, 0x0d, 0x00, 0x06, 0x09, 0x08, 0x07,
	 0x04, 0x0f, 0x0e, 0x03, 0x0b, 0x05, 0x02, 0x0c},
	{0x07, 0x0d, 0x0e, 0x03, 0x00, 0x06, 0x09, 0x0a,
	 0x01, 0x02, 0x08, 0x05, 0x0b, 0x0c, 0x04, 0x0f,
	 0x0d, 0x08, 0x0b, 0x05, 0x06, 0x0f, 0x00, 0x03,
	 0x04, 0x07, 0x02, 0x0c, 0x01, 0x0a, 0x0e, 0x09,
	 0x0a, 0x06, 0x09, 0x00, 0x0c, 0x0b, 0x07, 0x0d,
	 0x0f, 0x01, 0x03, 0x0e, 0x05, 0x02, 0x08, 0x04,
	 0x03, 0x0f, 0x00, 0x06, 0x0a, 0x01, 0x0d, 0x08,
	 0x09, 0x04, 0x05, 0x0b, 0x0c, 0x07, 0x02, 0x0e},
	{0x02, 0x0c, 0x04, 0x01, 0x07, 0x0a, 0x0b, 0x06,
	 0x08, 0x05, 0x03, 0x0f, 0x0d, 0x00, 0x0e, 0x09,
	 0x0e, 0x0b, 0x02, 0x0c, 0x04, 0x07, 0x0d, 0x01,
	 0x05, 0x00, 0x0f, 0x0a, 0x03, 0x09, 0x08, 0x06,
	 0x04, 0x02, 0x01, 0x0b, 0x0a, 0x0d, 0x07, 0x08,
	 0x0f, 0x09, 0x0c, 0x05, 0x06, 0x03, 0x00, 0x0e,
	 0x0b, 0x08, 0x0c, 0x07, 0x01, 0x0e, 0x02, 0x0d,
	 0x06, 0x0f, 0x00, 0x09, 0x0a, 0x04, 0x05, 0x03},
	{0x0c, 0x01, 0x0a, 0x0f, 0x09, 0x02, 0x06, 0x08,
	 0x00, 0x0d, 0x03, 0x04, 0x0e, 0x07, 0x05, 0x0b,
	 0x0a, 0x0f, 0x04, 0x02, 0x07, 0x0c, 0x09, 0x05,
	 0x06, 0x01, 0x0d, 0x0e, 0x00, 0x0b, 0x03, 0x08,
	 0x09, 0x0e, 0x0f, 0x05, 0x02, 0x08, 0x0c, 0x03,
	 0x07, 0x00, 0x04, 0x0a, 0x01, 0x0d, 0x0b, 0x06,
	 0x04, 0x03, 0x02, 0x0c, 0x09, 0x05, 0x0f, 0x0a,
	 0x0b, 0x0e, 0x01, 0x07, 0x06, 0x00, 0x08, 0x0d},
	{0x04, 0x0b, 0x02, 0x0e, 0x0f, 0x00, 0x08, 0x0d,
	 0x03, 0x0c, 0x09, 0x07, 0x05, 0x0a, 0x06, 0x01,
	 0x0d, 0x00, 0x0b, 0x07, 0x04, 0x09, 0x01, 0x0a,
	 0x0e, 0x03, 0x05, 0x0c, 0x02, 0x0f, 0x08, 0x06,
	 0x01, 0x04, 0x0b, 0x0d, 0x0c, 0x03, 0x07, 0x0e,
	 0x0a, 0x0f, 0x06, 0x08, 0x00, 0x05, 0x09, 0x02,
	 0x06, 0x0b, 0x0d, 0x08, 0x01, 0x04, 0x0a, 0x07,
	 0x09, 0x05, 0x00, 0x0f, 0x0e, 0x02, 0x03, 0x0c},
	{0x0d, 0x02, 0x08, 0x04, 0x06, 0x0f, 0x0b, 0x01,
	 0x0a, 0x09, 0x03, 0x0e, 0x05, 0x00, 0x0c, 0x07,
	 0x01, 0x0f, 0x0d, 0x08, 0x0a, 0x03, 0x07, 0x04,
	 0x0c, 0x05, 0x06, 0x0b, 0x00, 0x0e, 0x09, 0x02,
	 0x07, 0x0b, 0x04, 0x01, 0x09, 0x0c, 0x0e, 0x02,
	 0x00, 0x06, 0x0a, 0x0d, 0x0f, 0x03, 0x05, 0x08,
	 0x02, 0x01, 0x0e, 0x07, 0x04, 0x0a, 0x08, 0x0d,
	 0x0f, 0x0c, 0x09, 0x00, 0x03, 0x05, 0x06, 0x0b}
};

static const int nibblebit[4] = {8,4,2,1};
static const int bytebit[8] = {0x80, 0x40, 0x20, 0x10, 0x8, 0x4, 0x2, 0x1};
static const int totrot[16] = {1, 2, 4, 6, 8, 0xa, 0xc, 0xe, 0xf, 0x11, 0x13, 0x15, 0x17, 0x19, 0x1b, 0x1c};
	
static void permute (char *input, char b[][16][8], char *output)
{
	for (int i = 0; i < 8; i++)
	{
		output[i] = 0;
	}

	for (int i = 0; i < 0x10; i += 2)
	{
		int temp = ( (input[i>>1])>>4 ) & 0xF;
		temp += (i<<4);	//r0
		for (int j = 0; j < 8; j++)
		{
			char temp2 = b[0][temp][j];
			char temp3 = b[i+1][input[i>>1] & 0xF][j];
			output[j] = output[j] | temp2 | temp3;
		}
	}
}

static void endes(char *buf1, char *buf2)
{	//decrypt 8 bytes
	printf("STUB Encrypt\n");
}

static void dedes(char *buf1, char *buf2)
{	//decrypt 8 bytes
	char iters[17][8];	//r1_40
	char swap[8];	//r1_d0

	permute(buf1, iperm, iters[0]);

	for (int i = 0; i < 0x10; i++)
	{	
		iter(15-i, iters[i], iters[i+1]);		
	}
	swap[0] = iters[16][4];
	swap[1] = iters[16][5];
	swap[2] = iters[16][6];
	swap[3] = iters[16][7];
	swap[4] = iters[16][0];
	swap[5] = iters[16][1];
	swap[6] = iters[16][2];
	swap[7] = iters[16][3];

	permute(swap, fperm, buf2);
}

static void DesMem(char *buf, int length, int encrypt)
{
//	r30 = length;
//	r29 = encrypt;
//	r31 = buf;
	int i = 0;
	while (length > 0)
	{
		if (encrypt == 1)
		{
//			endes(&buf[i], &buf[i]);
		}
		else
		{
			dedes(&buf[i], &buf[i]);
		}
		i += 8;
		length -= 8;
	}
}

static void iter(int num, char *input, char *output)
{
	char fret[4] = {0,0,0,0};
	
	f(&input[4], num, fret);
	
//	printf("After f : ");
//	for (int i = 0; i < 4; i++)
//		printf("%02x", fret[i] & 0xFF);
//	printf(" *fret[4] @ 0x3d128\n");
	
	output[0] = input[4];
	output[1] = input[5];
	output[2] = input[6];
	output[3] = input[7];
	output[4] = input[0] ^ fret[0];
	output[5] = input[1] ^ fret[1];
	output[6] = input[2] ^ fret[2];
	output[7] = input[3] ^ fret[3];
}

static void f(char *right, int num, char *fret)
{
//	printf("expand:\n\tIN %02x%02x%02x%02x *r3[4] & 0x3d048\n",
//		right[0]&0xFF, right[1]&0xFF, right[2]&0xFF, right[3]&0xFF);
	
	char bigright[6];
	char result[6];
	char preout[4];
		
	expand(right, bigright);
	
//	printf("\tOUT: %02x%02x%02x%02x%02x%02x *bigright[6] @ 0x3d074\n",
//		bigright[0]&0xFF, bigright[1]&0xFF, bigright[2]&0xFF, 
//		bigright[3]&0xFF, bigright[4]&0xFF, bigright[5]&0xFF);
	
	result[0] = bigright[0] ^ kn[num][0];
	result[1] = bigright[1] ^ kn[num][1];
	result[2] = bigright[2] ^ kn[num][2];
	result[3] = bigright[3] ^ kn[num][3];
	result[4] = bigright[4] ^ kn[num][4];
	result[5] = bigright[5] ^ kn[num][5];
	contract(result, preout);
	
//	printf("contract output:\n\t%02x%02x%02x%02x *preout[4] & *0x3d0e0\n",
//		preout[0]&0xFF, preout[1]&0xFF, preout[2]&0xFF, preout[3]&0xFF);
	
	perm32(preout, fret);
}

static void expand(const char *right, char *bigright)
{
	bigright[0] = ((right[3]<<7) & 0x80) |
				  ((right[0]>>1) & 0x7C) |
				  ((right[0]>>3) & 0x03);

	bigright[1] = ((right[0]<<5) & 0xE0) | 
				  ((right[1]>>3) & 0x10) | 
				  ((right[0]<<3) & 0x08) | 
				  ((right[1]>>5) & 0x07);

	bigright[2] = ((right[1]<<3) & 0xC0) | 
				  ((right[1]<<1) & 0x3E) | 
				  ((right[2]>>7) & 0x01);
	
	bigright[3] = ((right[1]<<7) & 0x80) | 
				  ((right[2]>>1) & 0x7C) | 
				  ((right[2]>>3) & 0x03);
	
	bigright[4] = ((right[2]<<5) & 0xE0) | 
				  ((right[3]>>3) & 0x10) | 
				  ((right[2]<<3) & 0x08) | 
				  ((right[3]>>5) & 0x07);
	
	bigright[5] = ((right[3]<<3) & 0xC0) | 
				  ((right[3]<<1) & 0x3E) | 
				  ((right[0]>>7) & 0x01);
}

static void contract(char *in48, char *out32)
{
	int i;
	i = ((in48[0])<<4) & 0xFF0;
	i += ( (in48[1]>>4) & 0x0F );
	out32[0] = s[0][i];
	
	i = ( (in48[1]<<8) & 0xF00 ) +
		(in48[2] & 0xFF);
	out32[1] = s[1][i];
	
	i = ( (in48[3]<<4) & 0xFF0 ) + 
		( (in48[4]>>4) & 0x00F );
	out32[2] = s[2][i];

	i = ( (in48[4]<<8) & 0xF00 ) + 
		(in48[5] & 0xFF);
	out32[3] = s[3][i];
}

void DesReadBlock(char *buf, int length)
{
	DesMem(buf, length & 0xFFFFFFF8, 0);
}

void DesWriteBlock(char *buf, int length)
{
	DesMem(buf, length & 0xFFFFFFF8, 1);
}

static void perm32(char *input, char *output)
{
	output[0] = 0;
	output[1] = 0;
	output[2] = 0;
	output[3] = 0;
	for (int i = 0; i < 4; i++)
	{
		output[0] |= p32[i][input[i] & 0xFF][0];
		output[1] |= p32[i][input[i] & 0xFF][1];
		output[2] |= p32[i][input[i] & 0xFF][2];
		output[3] |= p32[i][input[i] & 0xFF][3];	
	}
}

static void perminit(char a[][16][8], const char *b)
{
	for (int k = 0; k < 0x10; k++)
	{
		for (int j = 0; j < 0x10; j++)
		{
			for (int i = 0; i < 8; i++)
			{
				a[k][j][i] = 0;
			}
		}
	}
	for (int k = 0; k < 0x10; k++)
	{
		for (int j = 0; j < 0x10; j++)
		{
			int b_offset = 0;	//r11
			for (int i = 0; i < 0x40; i++)
			{
				int temp;	//r0
				temp = (((unsigned char)b[i]) - 1)>>2;
				if (temp == k)
				{
					int temp2 = b[i] - 1;
					temp2 = nibblebit[temp2&3];
					if ((j & temp2) != 0)
					{
						char temp3 = bytebit[i & 7] & 0xFF;
						a[k][j][i>>3] |= temp3;
					}
				}
				b_offset++;
			}
		}
	}
	return;
}

static void kinit(char *key64bit)
{
	for (int i = 0; i < 56; i++)
	{
		char bob = key64bit[((pc1[i] - 1))>>3] & bytebit[(pc1[i] - 1) & 7] & 0xFF;
		if (bob > 0)
			pc1m[i] = 1;
		else
			pc1m[i] = 0;
	}
	for (int j = 0; j < 16; j++)
	{
		for (int i = 0; i < 6; i++)
		{
			kn[j][i] = 0;
		}
	}
	for (int j = 0; j < 16; j++)
	{
		for (int i = 0; i < 56; i++)
		{
			int temp;
			if (i > 0x1b)
			{
				temp = 0x37;
			}
			else
			{
				temp = 0x1b;
			}
			int temp2 = (totrot[j] + i);
			if (temp2 > temp)
			{
				temp = temp2 - 0x1c;
			}
			else
			{
				temp = temp2;
			}
			pcr[i] = pc1m[temp];
		}
		for (int i = 0; i < 48; i++)
		{	if (pcr[pc2[i] - 1] != 0)
			{	//3d3a4
				kn[j][i>>3] |= (bytebit[i&7] & 0xFF);
			}
		}
	}
}

static int getcomp(int a, int b)
{
	int temp1 = b & 1;
	int temp2 = (b>>4) & 2;
	temp2 |= temp1;
	int temp3 = (temp2<<4);
	temp3 += ((b>>1) & 0xF);
	return si[a][temp3];
}

static void sinit(void)
{
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 0x1000; j++)
		{
			int temp;
			temp = getcomp(i<<1, j>>6)<<4;
			temp |= (getcomp((i<<1)+1, j&0x3F) & 0xF);
			s[i][j] = temp;	//s[i][j]
		}
	}
//	for (int i = 0; i < 4; i++)
//	{
//		for (int j = 0; j < 0x1000; j++)
//		{
//			if ((j % 8) == 0)
//			{
//				int offset = i*0x1000 + j;
//				printf("0x%06x <s", 0x48f764+offset);
//				if (offset > 0)
//				{
//					printf("+%d", offset);
//				}
//				printf(">:");
//			}
//			printf("\t0x%02x", s[i][j] & 0xFF);
//			if ((j%8) == 7)
//				printf("\n");
//		}
//	}
}

static void p32init(void)
{
	for (int k = 0; k < 4; k++)
	{
		for (int j = 0; j < 0x100; j++)
		{
			for (int i = 0; i < 4; i++)
			{
				p32[k][j][i] = 0;
			}
		}
	}
	for (int k = 0; k < 4; k++)
	{
		for (int j = 0; j < 0x100; j++)
		{
			for (int i = 0; i < 0x20; i++)
			{
				int temp = p32i[i] - 1;	//r2
				if ((temp>>3) == k)
				{
					if ((j & bytebit[temp&7]) != 0)
					{
						p32[k][j][i>>3] |= ((bytebit[i&7])&0xFF);	//byte from integer
					}
				}
			}
		}
	}
}

void DesKeyInit(const char *key)
{
	char key64bit[8] = {0,0,0,0,0,0,0,0};
	if (key[0] != 0)
	{
		for (int i = 0; i < 0x28; i++)
		{	//only use 0x28 bytes of the key
			key64bit[i%8] ^= key[i];
			if (key[i + 1] == 0)
			{
				break;
			}
		}
	}
	perminit(iperm, ip);
	perminit(fperm, fp);
	kinit(key64bit);
	sinit();
	p32init();
}